/*
 * Copyright (c) 2006-2022, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Date           Author       Notes
 * 2020-01-15     bigmagic     the first version
 * 2020-08-10     SummerGift   support clang compiler
 */

.section ".text.boot","ax"
.global _start
.extern main

_start:
    /* get cpu id from mpidr_el1 register */
    mrs x0, mpidr_el1   /* MPIDR_EL1: Multi-Processor Affinity Register */
    and x0, x0, #15     /* get cpu id */
    msr tpidr_el1, x0   /* TPID_EL1: Thread ID Register */

    mrs x0, tpidr_el1
    cbz x0, .cpu_0  /* if cpu id == 0: goto .cpu_0 */

.cpu_idle:
    wfe /* Wait For Event */
    b .cpu_idle

.cpu_0:
    /* set stack before our code, Define stack pointer for current exception level */
    adr x1, _start

    /* get current EL */
    mrs x0, CurrentEL   /* CurrentEL Register. bit 2, 3 are Exception level. Others reserved */
    and x0, x0, #12     /* clear reserved bits */

    /* if current EL != 1100b(11 means EL3): goto .not_in_el3 */
    cmp x0, #12
    bne .not_in_el3

    /* should never be executed, just for completeness. (EL3) */
    mov x2, #0x5b1
    msr scr_el3, x2             /* SCR_ELn  Secure Configuration Register */
    mov x2, #0x3c9
    msr spsr_el3, x2            /* SPSR_ELn. Saved Program Status Register. 1111001001 */
    adr x2, .not_in_el3
    msr elr_el3, x2
    eret                            /* Exception Return: from EL3, continue from .not_in_el3 */

.not_in_el3:                     /* running at EL2 or EL1 */
    cmp     x0, #4                  /* 0x04  0100 EL1 */
    beq     .L__in_el1

    /* in EL2 */
    mrs     x0, hcr_el2
    bic     x0, x0, #0xff
    msr     hcr_el2, x0

    msr     sp_el1, x1              /* in EL2, set sp of EL1 to _start */

    /* enable CNTP for EL1 */
    mrs     x0, cnthctl_el2         /* Counter-timer Hypervisor Control register */
    orr     x0, x0, #3
    msr     cnthctl_el2, x0
    msr     cntvoff_el2, xzr

    /* enable AArch64 in EL1 */
    mov     x0, #(1 << 31)          /* AArch64 */
    orr     x0, x0, #(1 << 1)       /* SWIO hardwired on Pi3 */
    msr     hcr_el2, x0
    mrs     x0, hcr_el2

    /* change execution level to EL1 */
    mov     x2, #0x3c4
    msr     spsr_el2, x2            /* 1111000100 */
    adr     x2, .L__in_el1
    msr     elr_el2, x2

    eret                            /* exception return. from EL2. continue from .L__in_el1 */

.L__in_el1:
    mov     x9, #0
    mov     sp, x1                  /* in EL1. Set sp to _start */

    /* Set CPACR_EL1 (Architecture Feature Access Control Register) to avoid trap from SIMD or float point instruction */
    mov     x1, #0x00300000         /* Don't trap any SIMD/FP instructions in both EL0 and EL1 */
    msr     cpacr_el1, x1

.L__jump_to_entry:          /* jump to C code, should not return */      

    mov     x0, #1
    msr     spsel, x0
    adr     x1, _start
    mov     sp, x1           /* sp_el1 set to _start */

    b  main